#include <constants.h>

#define CELL_RESOLUTION 0.1
#define ROBOT_RADIUS 0.2

// Function prototypes
map_data_type discretizedGlobalMapFromServer(int x1, int y1, int x2, int y2);
bool publishMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
bool publishMapVersion();
void queryMapServer(int x1, int y1, int x2, int y2);
map_data_type discretizeMap(const map_data_type &m, int x1, int y1, int x2, int y2);
map_data_type inflateMap(map_data_type cells, int x1, int y1, int x2, int y2);
void updateMap();
void visualizeMap();
map_data_type transpose(const map_data_type &map);
bool mapsDiffer(const map_data_type &m1, const map_data_type &m2);
void subscribeToLaserData();
void receivedLaserScan(const sensor_msgs::LaserScan::ConstPtr &msg);

template<typename T>
T min(T a, T b) {
	return a < b ? a : b;
}
template<typename T>
T max(T a, T b) {
	return a > b ? a : b;
}

// Global variables
map_data_type map, cells, laserMap;
int width, height;
float resolution;
int cellsWidth, cellsHeight;
geometry_msgs::Pose origin;
ros::Time lastModifiedTime;
int mapVersion = 0;
int maxNPoolLaserMsgs = 10;
int acceptIfNPoolLaserMsgs = 8; // accept a laserpoint if it appears
// in 8/10 consecutive laser-msgs

int main(int argc, char **argv) {
	int x1 = 0, x2 = 100, y1 = 0, y2 = 100;
	if (argc < 5) {
		//std::cerr << "Usage: " << argv[0] << " x1 y1 x2 y2" << std::endl;
		//return 1;

	}

	ros::init(argc, argv, "getMap");
	ros::NodeHandle nh;

	//int x1 = atoi(argv[1]), y1 = atoi(argv[2]), x2 = atoi(argv[3]), y2 = atoi(argv[4]);

	// Get map from server
	cells = discretizedGlobalMapFromServer(x1, y1, x2, y2);
	std::cout << "Received discretizedGlobalMapFromServer" << std::endl;
	cells = inflateMap(cells, x1, y1, x2, y2);

	// Subscribe to laser-data
	laserMap.insert(laserMap.end(), map.size(), 0);
//	ros::Subscriber sub = nh.subscribe("/base_scan", 100, receivedLaserScan);

	// Advertise service on /map_cells, and wait for calls
	ros::ServiceServer ss = nh.advertiseService("/map_cells", publishMap);
	ROS_INFO("Service callback setup");

	ros::Rate loopRate(10);

	while (ros::ok()) {
		publishMapVersion();
		ros::spinOnce();

		loopRate.sleep();
	}

	return 0;

}

/*********************************
 * discretizedGlobalMapFromServer:
 *		INPUTS: none
 *		OUTPUTS:
 *			cells (map_data_type): contains the occupancy grid
 *				'0' for free space
 *				'1' for non-free space
 *
 *
 *		This function first asks the map server for the map
 *		It then discretizes it, based on CELL_RESOLUTION
 *		and returns it to the calling fnc.
 *********************************/
map_data_type discretizedGlobalMapFromServer(int x1, int y1, int x2, int y2) {

	queryMapServer(x1, y1, x2, y2);
	cells = discretizeMap(map, x1, y1, x2, y2);

	return cells;

}

/*********************************
 * queryMapServer:
 *		INPUTS: none
 *		OUTPUTS: none
 *
 *		This function queries the map server for the map.
 *********************************/
void queryMapServer(int x1, int y1, int x2, int y2) {
	sleep(2); //Launching with the map_server, so wait to make sure its up
	ros::NodeHandle nh;
	ros::ServiceClient sc = nh.serviceClient<nav_msgs::GetMap> ("/static_map");

	/* Get map from map_server via service request */
	nav_msgs::GetMap srv;
	if (sc.call(srv)) {
		width = srv.response.map.info.width;
		height = srv.response.map.info.height;
		origin = srv.response.map.info.origin;
		resolution = srv.response.map.info.resolution;
		map = srv.response.map.data;
		//map = transpose(map);
	} else {
		ROS_ERROR("Could not get map from map server. Is it not started, perhaps?");
		return;
	}

	lastModifiedTime = ros::Time::now();

	// debugging

	std::cout << "Map from server" << std::endl;
	ROS_INFO("The dimensions of the map are: %d %d at %0.2f", height, width, resolution);
	ROS_INFO("The dimensions of the map are: %d", map.size());

	for (int i = 0; i < height; ++i) {
		if (i < x1 || i > x2)
			continue;
		for (int j = 0; j < width; ++j) {
			if (j < y1 || j > y2)
				continue;
			if (map[i * width + j] == -1)
				map[i * width + j] = 2;
			if (map[i * width + j] == 100)
				map[i * width + j] = 1;
			std::cout << (int) map[i * width + j] << " ";
		}
		std::cout << std::endl;
	}

}

map_data_type transpose(const map_data_type &map) {
	map_data_type transposed(map.size());

	for (int i = 0; i < height; ++i) {
		for (int j = 0; j < width; ++j) {
			int mIndex = i * width + j;
			int tIndex = j * width + i;
			transposed[tIndex] = map[mIndex];
		}
	}

	return transposed;
}

/*********************************
 * discretizeMap:
 *		INPUTS:
 *			m (const map_data_type &): map to be discretized
 *		OUTPUTS:
 *			cells (map_data_type): contains the occupancy grid
 *				'0' for free space
 *				'1' for non-free space
 *
 *
 *		This function downsamples the map
 *		into CELL_RESOLUTIONxCELL_RESOLUTION grids.
 *		It does this by checking if any point in the
 *		grid is blocked, and if so, calls the entire
 *		cell as blocked i.e. it is very conservative.
 *********************************/
map_data_type discretizeMap(const map_data_type &m, int x1, int y1, int x2, int y2) {

	int windowSize = ceil(CELL_RESOLUTION / resolution);
	cellsWidth = width / windowSize;
	cellsHeight = height / windowSize;

	// make new storage for our discretized cells
	map_data_type c(cellsWidth * cellsHeight);

	// for each point
	for (int i = 0; i < cellsHeight; ++i) {
		for (int j = 0; j < cellsWidth; ++j) {

			// check if any point in its neighborhood is blocked
			bool occupied = false;
			for (int k = 0; k < windowSize; ++k) {
				for (int l = 0; l < windowSize; ++l) {
					int index = (i * windowSize + k) * width + (j * windowSize + l);
					if (m[index] != 0) {
						occupied = true;
					}
				}
			}
			if (occupied) {
				c[i * cellsWidth + j] = 1;
			} else {
				c[i * cellsWidth + j] = 0;
			}
		}
	}

	// debugging info
	/*
	 std::cout << "Discretized map" << std::endl;
	 for (int i=0; i<cellsHeight; ++i)	{
	 if (i < x1/windowSize || i > x2/windowSize) continue;
	 for (int j=0; j<cellsWidth; ++j)	{
	 if (j < y1/windowSize || j > y2/windowSize) continue;
	 if (c[i*cellsWidth+j] == -1)	c[i*cellsWidth+j] = 2;
	 if (c[i*cellsWidth+j] == 100)	c[i*cellsWidth+j] = 1;
	 std::cout << (int) c[i*cellsWidth+j] << " ";
	 }
	 std::cout << std::endl;
	 }
	 */

	return c;
}

void visualizeMap() {

	// rviz
	static ros::NodeHandle nh;
	static visualization_msgs::Marker marker;
	static ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker> (
			"visualization_marker", 10);

	marker.points.clear();
	marker.header.frame_id = "/map";
	marker.header.stamp = ros::Time::now();
	marker.ns = "occupancyGrid";
	marker.id = 0;
	marker.type = visualization_msgs::Marker::POINTS;
	marker.action = visualization_msgs::Marker::ADD;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;
	marker.scale.x = CELL_RESOLUTION;
	marker.scale.y = CELL_RESOLUTION;
	marker.scale.z = CELL_RESOLUTION;
	marker.color.r = 0.0f;
	marker.color.g = 0.0f;
	marker.color.b = 1.0f;
	marker.color.a = 1.0;
	geometry_msgs::Point p;

	for (int i = 0; i < cellsHeight; ++i) {
		for (int j = 0; j < cellsWidth; ++j) {
			if (cells[i * cellsWidth + j] == 1) {

				bool edge = false;
				for (int k = max(0, i - 1); k <= min(cellsHeight, i + 1); ++k) {
					for (int l = max(0, j - 1); l <= min(cellsWidth, j + 1); ++l) {
						if (cells[k * cellsWidth + l] == 0) {
							edge = true;
						}
					}
				}
				if (edge) {
					p.x = j * CELL_RESOLUTION;
					p.y = i * CELL_RESOLUTION;
					p.z = 0;
					marker.points.push_back(p);
				}
			}
		}
	}

	marker_pub.publish(marker);

}

map_data_type inflateMap(map_data_type cells, int x1, int y1, int x2, int y2) {

	map_data_type inflated(cells.size());
	std::cout << "Size of inflated: " << inflated.size() << std::endl;
	std::cout << cellsHeight << " " << cellsWidth << std::endl;

	int robotRadius = ROBOT_RADIUS / CELL_RESOLUTION;
	for (int i = 0; i < cellsHeight; ++i) {
		for (int j = 0; j < cellsWidth; ++j) {

			for (int k = max(0, i - robotRadius); k <= min(cellsHeight, i + robotRadius); ++k) {
				for (int l = max(0, j - robotRadius); l <= min(cellsWidth, j + robotRadius); ++l) {
					int index = k * cellsWidth + l;
					//std::cout << "Examining " << index << std::endl;
					//std::cout << i << " " << k << " " << j << " " << l << ": " << index << std::endl;
					if (cells[index] != 0) {
						inflated[i * cellsWidth + j] = 1;
					}
				}
			}
		}
	}

	// debugging info
	int windowSize = ceil(CELL_RESOLUTION / resolution);
	std::cout << "Inflated map" << std::endl;
	for (int i = 0; i < cellsHeight; ++i) {
		if (i < x1 / windowSize || i > x2 / windowSize)
			continue;
		for (int j = 0; j < cellsWidth; ++j) {
			if (j < y1 / windowSize || j > y2 / windowSize)
				continue;
			if (inflated[i * cellsWidth + j] == -1)
				inflated[i * cellsWidth + j] = 2;
			if (inflated[i * cellsWidth + j] == 100)
				inflated[i * cellsWidth + j] = 1;
			std::cout << (int) inflated[i * cellsWidth + j] << " ";
		}
		std::cout << std::endl;
	}

	return inflated;
}

void receivedLaserScan(const sensor_msgs::LaserScan::ConstPtr &msg) {

	static int nPoolLaserMsgs = 0;

	static ros::NodeHandle nh;
	static tf::TransformListener listener;
	geometry_msgs::PointStamped laser_point;
	laser_point.header.frame_id = "/base_laser_link";
	laser_point.header.stamp = ros::Time();

	geometry_msgs::PointStamped map_point;

	// rviz
	static visualization_msgs::Marker marker;
	static ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker> (
			"visualization_marker", 10);
	marker.points.clear();
	marker.header.frame_id = "/map";
	marker.header.stamp = ros::Time::now();
	marker.ns = "occupancyGrid";
	marker.id = 2;
	marker.type = visualization_msgs::Marker::POINTS;
	marker.action = visualization_msgs::Marker::ADD;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;
	marker.scale.x = 0.2;
	marker.scale.y = 0.2;
	marker.scale.z = 0.2;
	marker.color.r = 0.0f;
	marker.color.g = 1.0f;
	marker.color.b = 0.0f;
	marker.color.a = 1.0;

	float maxRange = msg->range_max;
	float minRange = msg->range_min;
	float epsilon = 0.01;

	// transform every point into the map frame
	int ctr = 0;
	for (float angle = msg->angle_min; angle <= msg->angle_max; angle += msg->angle_increment) {
		float distance = msg->ranges[ctr++];

		if (distance < minRange || distance > maxRange) { // spurious: ignore scan msg.
			continue;
		}

		if (abs(distance - maxRange) < epsilon) { // no obstacle in sight
			continue;
		}

		float r = distance, theta = angle;
		float x = r * cos(theta), y = r * sin(theta);

		laser_point.point.x = x;
		laser_point.point.y = y;
		laser_point.point.z = 0;

		try {
			//listener.waitForTransform("/map", "/base_laser_link", ros::Time::now(), ros::Duration(1));
			listener.transformPoint("/map", laser_point, map_point);
			// Publish to RVIZ
			geometry_msgs::Point p;
			p.x = map_point.point.x;
			p.y = map_point.point.y;
			p.z = 0;
			marker.points.push_back(p);

			//std::cout << "Tranforming [" << p.y << ", " << p.x << "] to "
			//					<< "[" << int(p.y/resolution) << ", " << int(p.x/resolution) << "] == "
			//					<< int(p.y/resolution)*width + int(p.x/resolution) << std::endl;
			laserMap[int(p.y / resolution) * width + int(p.x / resolution)]++;

		} catch (tf::TransformException& ex) {
			ROS_WARN(
					"Received an exception trying to transform a point from /base_laser_link to /map: %s",
					ex.what());
		}

	}

	// pool over maxNPoolLaserMsgs to overcome noisy laser-data
	++nPoolLaserMsgs;
	if (nPoolLaserMsgs == maxNPoolLaserMsgs) { // done pooling one batch of laser-msgs
		for (int i = 0; i < laserMap.size(); ++i) {
			if (laserMap[i] > acceptIfNPoolLaserMsgs) {
				laserMap[i] = 1;
			} else {
				laserMap[i] = 0;
			}
		}

		// update the map
		updateMap();

		// reset pooling parameters
		nPoolLaserMsgs = 0;
		laserMap.clear();
		laserMap.insert(laserMap.end(), map.size(), 0);

	}

}

void updateMap() {

	// merge
	map_data_type mergedMap(map.size(), 0);
	for (int i = 0; i < height; ++i) {
		for (int j = 0; j < width; ++j) {
			int index = i * width + j;
			if (map[index] || laserMap[index]) {
				mergedMap[index] = 1;
			}
		}
	}

	map_data_type newcells = discretizeMap(mergedMap, 0, 0, 10, 10);

	if (mapsDiffer(cells, newcells)) {

		ROS_INFO("maps differ");
		cells = newcells;
		++mapVersion;
		visualizeMap();

	}

}

bool mapsDiffer(const map_data_type &m1, const map_data_type &m2) {
	return m1 != m2;
}

/*********************************
 * publishMap:
 *		INPUTS: usual req and response
 *		OUTPUTS: none
 *
 *		This fnc. publishes a map when a service
 *		on /map_cells is requested. It returns
 *		a message of type nav_msgs/OccupancyGrid
 *		http://www.ros.org/doc/api/nav_msgs/html/srv/GetMap.html
 *
 *********************************/
bool publishMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) {

	res.map.header.seq = mapVersion;
	res.map.header.stamp = ros::Time::now();
	res.map.header.frame_id = "what";

	res.map.info.map_load_time = lastModifiedTime;
	res.map.info.width = cellsWidth;
	res.map.info.height = cellsHeight;
	res.map.info.resolution = CELL_RESOLUTION;
	res.map.info.origin = origin;

	res.map.data = cells;

	ROS_INFO("got request... replying");
	return true;

}

bool publishMapVersion() {

	ros::NodeHandle nh;

	static ros::Publisher pub = nh.advertise<std_msgs::Int32> ("/map_version", 10);

	static std_msgs::Int32 msg;
	msg.data = mapVersion;

	pub.publish(msg);

	return true;

}
